#include "gnss/gnss_data.hpp"
#include "glog/logging.h"
#include <math.h>

double GNSSData::origin_longitude = 0.0;
double GNSSData::origin_latitude = 0.0;
double GNSSData::origin_altitude = 0.0;
bool GNSSData::origin_position_inited = false;
GeographicLib::LocalCartesian GNSSData::geo_converter;

void GNSSData::InitOriginPosition() {
    geo_converter.Reset(latitude, longitude, altitude);

    origin_longitude = longitude;
    origin_latitude = latitude;
    origin_altitude = altitude;

    origin_position_inited = true;
}

void GNSSData::UpdateXYZ() {
    if (!origin_position_inited) {
        LOG(WARNING) << "GeoConverter has not set origin position";
    }
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}

void GNSSData::format_GNSSData(){
    double floor_data = std::floor(longitude);
    double remainder_data = longitude - floor_data;
    longitude = floor_data + (remainder_data * 100 / 60);

    floor_data = std::floor(latitude);
    remainder_data = latitude - floor_data;
    latitude = floor_data + (remainder_data * 100 / 60);
}

bool GNSSData::SyncData(std::deque<GNSSData>& UnsyncedData, GNSSData& synced_data, double sync_time, 
            double &front_time , double &back_time) {
    while (UnsyncedData.size() >= 2) {
        /* find front data almost near synced_data */
        if (UnsyncedData.front().time > sync_time)
            return false;
        if (UnsyncedData.at(1).time < sync_time) {
            UnsyncedData.pop_front();
            continue;
        }

        // /* find back data almost near synced_data */
        // if (sync_time - UnsyncedData.front().time > 0.2) {
        //     UnsyncedData.pop_front();
        //     break;
        // }
        // if (UnsyncedData.at(1).time - sync_time > 0.2) {
        //     UnsyncedData.pop_front();
        //     break;
        // }
        break;
    }
    if (UnsyncedData.size() < 2){
        GNSSData current_data = UnsyncedData.at(0);

        current_data.format_GNSSData();
        
        if(origin_position_inited == false){
            current_data.InitOriginPosition();
        }
        current_data.UpdateXYZ();
        synced_data.time = sync_time;
        synced_data.status = current_data.status;
        synced_data.longitude = current_data.longitude;
        synced_data.latitude = current_data.latitude;
        synced_data.altitude = current_data.altitude;

        front_time = current_data.time;
        back_time  = current_data.time;

        LOG(INFO) << "--------- only 1 gps unsyncedData ";

        return true;
    }

    GNSSData front_data = UnsyncedData.at(0);
    GNSSData back_data = UnsyncedData.at(1);
    front_data.format_GNSSData();
    back_data.format_GNSSData();

    if(origin_position_inited == false){
        front_data.InitOriginPosition();
    }
    front_data.UpdateXYZ();
    back_data.UpdateXYZ();

    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;
    synced_data.status = back_data.status;
    synced_data.longitude = front_data.longitude * front_scale + back_data.longitude * back_scale;
    synced_data.latitude = front_data.latitude * front_scale + back_data.latitude * back_scale;
    synced_data.altitude = front_data.altitude * front_scale + back_data.altitude * back_scale;
    synced_data.local_E = front_data.local_E * front_scale + back_data.local_E * back_scale;
    synced_data.local_N = front_data.local_N * front_scale + back_data.local_N * back_scale;
    synced_data.local_U = front_data.local_U * front_scale + back_data.local_U * back_scale;

    front_time = front_data.time;
    back_time = back_data.time;
    return true;
}
